///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2025, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

// ZED include
#include "ClientPublisher.hpp"
#include "GLViewer.hpp"
#include "utils.hpp"

#define BUILD_MESH 1

int main(int argc, char **argv) {
	if (argc != 2) {
		// this file should be generated by using the tool ZED360
		std::cout << "Need a Configuration file in input" << std::endl;		
		return 1;
	}

    // Defines the Coordinate system and unit used in this sample
    constexpr sl::COORDINATE_SYSTEM COORDINATE_SYSTEM = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP;
    constexpr sl::UNIT UNIT = sl::UNIT::METER;

    // Read json file containing the configuration of your multicamera setup.    
	std::string json_config(argv[1]);
    auto configurations = sl::readFusionConfigurationFile(json_config, COORDINATE_SYSTEM, UNIT);

    if (configurations.empty()) {
        std::cout << "Empty configuration File." << std::endl;
        return EXIT_FAILURE;
    }

    // Check if the ZED camera should run within the same process or if they are running on the edge.
    std::vector<ClientPublisher> clients(configurations.size());
    int id_ = 0;
    std::map<int, std::string> svo_files;
    for (auto conf: configurations) {
        // if the ZED camera should run locally, then start a thread to handle it
        if(conf.communication_parameters.getType() == sl::CommunicationParameters::COMM_TYPE::INTRA_PROCESS){
            std::cout << "Try to open ZED " <<conf.serial_number << ".." << std::flush;
            auto state = clients[id_].open(conf.input_type);

            if (!state) {
                std::cerr << "Could not open ZED: " << conf.input_type.getConfiguration() << ". Skipping..." << std::endl;
                continue;
            }

            if (conf.input_type.getType() == sl::InputType::INPUT_TYPE::SVO_FILE)
                svo_files.insert(std::make_pair(id_, conf.input_type.getConfiguration()));

            std::cout << ". ready !" << std::endl;

            id_++;
        }
    }

    // Synchronize SVO files in SVO mode
    bool enable_svo_sync = (svo_files.size() > 1);
    if (enable_svo_sync) {
        std::cout << "Starting SVO sync process..." << std::endl;
        std::map<int, int> cam_idx_to_svo_frame_idx = syncDATA(svo_files);

        for (auto &it : cam_idx_to_svo_frame_idx) {
            std::cout << "Setting camera " << it.first << " to frame " << it.second << std::endl;
            clients[it.first].setStartSVOPosition(it.second);
        }
    }

    // start camera threads
    for (auto &it: clients)
        it.start();

    // Now that the ZED camera are running, we need to initialize the fusion module
    sl::InitFusionParameters init_params;
    init_params.coordinate_units = UNIT;
    init_params.coordinate_system = COORDINATE_SYSTEM;
    init_params.verbose = true;

    // create and initialize it
    sl::Fusion fusion;
    auto state = fusion.init(init_params);
    if(state != sl::FUSION_ERROR_CODE::SUCCESS){
        std::cout<<"ERROR Init "<<state<<std::endl;
        return 1;
    }

    // subscribe to every cameras of the setup to internally gather their data
    std::vector<sl::CameraIdentifier> cameras;
    for (auto& it : configurations) {
        sl::CameraIdentifier uuid(it.serial_number);
        // to subscribe to a camera you must give its serial number, the way to communicate with it (shared memory or local network), and its world pose in the setup.        
        auto state = fusion.subscribe(uuid, it.communication_parameters, it.pose, it.override_gravity);
        if (state != sl::FUSION_ERROR_CODE::SUCCESS)
            std::cout << "Unable to subscribe to " << std::to_string(uuid.sn) << " . " << state << std::endl;
        else
            cameras.push_back(uuid);
    }

    // check that at least one camera is connected
    if (cameras.empty()) {
        std::cout << "no connections " << std::endl;
        return EXIT_FAILURE;
    }

    sl::PositionalTrackingFusionParameters positional_tracking_param;
    state = fusion.enablePositionalTracking(positional_tracking_param);
    if(state != sl::FUSION_ERROR_CODE::SUCCESS){
        std::cout<<"ERROR PositionalTracking "<<state<<std::endl;
        return 1;
    }

    // creation of a 3D viewer
    GLViewer viewer;
    viewer.init(argc, argv);

    sl::SpatialMappingFusionParameters spatial_mapping_parameters;
#if BUILD_MESH
    spatial_mapping_parameters.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::MESH;
    sl::Mesh map;
#else
    spatial_mapping_parameters.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD;
    sl::FusedPointCloud map;
#endif

    // Set mapping range, it will set the resolution accordingly (a higher range, a lower resolution)
    spatial_mapping_parameters.set(sl::SpatialMappingParameters::MAPPING_RANGE::SHORT);
    spatial_mapping_parameters.set(sl::SpatialMappingParameters::MAPPING_RESOLUTION::HIGH);
    // Request partial updates only (only the last updated chunks need to be re-draw)
    spatial_mapping_parameters.use_chunk_only = true;
    // Stability counter defines how many times a stable 3D points should be seen before it is integrated into the spatial mapping
    spatial_mapping_parameters.stability_counter = 4;

    state = fusion.enableSpatialMapping(spatial_mapping_parameters);
    if(state != sl::FUSION_ERROR_CODE::SUCCESS){
        std::cout<<"ERROR Spatial Mapping "<<state<<std::endl;
        return 1;
    }

    sl::Timestamp last_update = 0;
    bool wait_for_mesh = false;    

    // run the fusion as long as the viewer is available.
    while (viewer.isAvailable()) {
        // run the fusion process (which gather data from all camera, sync them and process them)
        if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) {

            auto ts = sl::getCurrentTimeStamp();
            if(!wait_for_mesh && (ts.getMilliseconds() - last_update.getMilliseconds() > 100 )){
                fusion.requestSpatialMapAsync();
                wait_for_mesh =true;
            }

            if(wait_for_mesh && fusion.getSpatialMapRequestStatusAsync() == sl::FUSION_ERROR_CODE::SUCCESS){
                fusion.retrieveSpatialMapAsync(map);
                // update the 3D view
                viewer.updateMap(map);
                wait_for_mesh = false;
                last_update = ts;
            }
        }
    }

    viewer.exit();

    map.save("MyMap.ply", sl::MESH_FILE_FORMAT::PLY);

    for (auto &it: clients)
        it.stop();

    fusion.close();

    return EXIT_SUCCESS;
}
